

/**
 * @file CorrectObservablesLEO.cpp
 * This class corrects observables from effects such as antenna excentricity,
 * difference in phase centers, offsets due to tide effects, etc.
 */

#include "CorrectObservablesLEO.hpp"

using namespace std;

#define debug 1

namespace gpstk
{

    // Return a string identifying this object.
    std::string CorrectObservablesLEO::getClassName() const
    { return "CorrectObservablesLEO"; }



      /* Return a satTypeValueMap object, adding the new data generated
       * when calling this object.
       *
       * @param time      Epoch corresponding to the data.
       * @param gData     Data object holding the data.
       */
    satTypeValueMap& CorrectObservablesLEO::Process( const CommonTime& time,
                                                        satTypeValueMap& gData )
       noexcept(false)
    {

        try
        {

            Triple rxPos( nominalPos.X(), nominalPos.Y(), nominalPos.Z() );

            // Define a Triple that will hold satellite position, in ECEF
            Triple svPos(0.0, 0.0, 0.0);

            SatIDSet satRejectedSet;

            // Loop through all the satellites
            for(satTypeValueMap::iterator it = gData.begin();
                it != gData.end();
                ++it)
            {
                SatID sat( (*it).first );

                // Use ephemeris if satellite position is not already computed
                if( ( (*it).second.find(TypeID::satXECF) == (*it).second.end() ) ||
                    ( (*it).second.find(TypeID::satYECF) == (*it).second.end() ) ||
                    ( (*it).second.find(TypeID::satZECF) == (*it).second.end() ) )
                {
                    satRejectedSet.insert( sat );
                    continue;
                }
                else
                {
                    // Get satellite position out of GDS
                    svPos[0] = (*it).second[TypeID::satXECF];
                    svPos[1] = (*it).second[TypeID::satYECF];
                    svPos[2] = (*it).second[TypeID::satZECF];
                }

                // Compute vector RX-SV, in ECEF
                Triple ray(rxPos - svPos);

                // Convert ray to an unitary vector
                ray = ray.unitVector();


                double corrL1( offsetL1.dot(ray) );
                double corrL2( offsetL2.dot(ray) );
  

                // Find which observables are present, and then
                // apply corrections

                (*it).second[TypeID::rcvCorr1] = corrL1;
                (*it).second[TypeID::rcvCorr2] = corrL2;

                if(debug)
                {
                    std::cout << "Sat: " << (*it).first << " ";
                    std::cout << "corrL1: " << corrL1 << " ";
                    std::cout << "corrL2: " << corrL2 << std::endl;
                }

            } // End of 'for(satTypeValueMap::iterator = it, ...)'


            // Remove satellites with missing data
            gData.removeSatID(satRejectedSet);

            return gData;

        }
        catch(Exception& u)
        {
            // Throw an exception if something unexpected happens
            ProcessingException e( getClassName() + ":" + u.what() );

            GPSTK_THROW(e);
        }

    }  // End of method 'CorrectObservablesLEO::Process()'


}  // End of namespace gpstk
